/* ***********************************************************************
 * img_stab_if_R.cpp
 * ABOUT : Implement the class of Image stabilization.
 * AUTHOR : Dilawar Singh
 * Created on : April 15, 2010
 * Modified log :
 April 15, 2010
 April 28, 2010
 June 09, 2010 : Modified for adjusting cropping parameters.


 (c) Dilawar Singh, 2010
 *************************************************************************/

#include "types.h"
#include "img_stab_if_R.h"
#include <iostream>
#include <vector>
#include <cmath>

#define IS_DEBUG 0
#define IS_TEST 0


extern "C" void prints(char *format,...);

img_stab_if_R * img_stab_if_R::pInstance = NULL;

video_handler* img_stab_if_R::pBaseVideoHandler;

/*
 * Constructor.
 */
img_stab_if_R::img_stab_if_R()
{
#if IS_DEBUG
	prints("\n\r Enterning IS constructor... \n\r");
#endif
    for(int i=0; i < 2*QUAT_LENGTH; i++)
    {
		arrayTwoQuaternion[i] = 0;
    }
	myParamVec.resize(4*WINDOW_SIZE);
	while(myParamVec.size() <= 4*WINDOW_SIZE)
	{
		myParamVec.push_back(0);
	}

	// initialize vector.
	myFilteredVec.resize(4);
    myFilteredVec[0]= 0;
    myFilteredVec[1]= 0;
    myFilteredVec[2]= 0;
    myFilteredVec[3]= 0;


    pObjCamMotionManager =  cam_motion_mgr::getInstance();
}

img_stab_if_R::~img_stab_if_R()
{

#if IS_DEBUG
	prints("\n\r Enterning IS destructor... \n\r");
#endif
    if(pObjCamMotionManager)
		delete pObjCamMotionManager;
}

img_stab_if_R *img_stab_if_R::getInstance()
{
	if(!pInstance)
	{
		pInstance = new img_stab_if_R();
	}
	return pInstance;
}

//Set Base Interface at startup
int img_stab_if_R::initInterface(video_handler* pVideoHandler)
{
    pBaseVideoHandler = pVideoHandler;
    return(true);
}
/*
  img_stab_if_R::img_stab_if_R(kalmanType *pArrayQuat, kalmanType *pArrayCamMat)
  :  myParamVec(4*WINDOW_SIZE), myFilteredVec(4), myVecS(WINDOW_SIZE)
  {
  for(int i = 0; i < 2*QUAT_LENGTH; i++)
  {
  arrayTwoQuaternion[i] = *pArrayQuat;
  pArrayQuat++;
  }

  for(int i=0; i < (2*K_MATRIX_LENGTH); i++)
  {
  arrayCamInternalMatrix[i] = *pArrayCamMat;
  pArrayCamMat++;
  }

  //    pObjCamMotionManager = new cam_motion_mgr();
  }
  */
/*
 * This function will accumulate the parameter in a queue.
 */
void img_stab_if_R::accParamsInQueue(kalmanType *pMyArray)
{
	kalmanType *pA = pMyArray;
#if IS_DEBUG
	prints("\n\r Enterning accParamInQueue ...");
#endif
#if IS_DEBUG
	for(vector<kalmanType>::size_type  i =0; i < img_stab_if_R::myParamVec.size() ; i++)
    {
		prints("\n\r Vec[%d]: %lf", i, img_stab_if_R::myParamVec[i]);
    }
#endif

    if(img_stab_if_R::myParamVec.size() > 4*WINDOW_SIZE)
    {
		while(img_stab_if_R::myParamVec.size() > 4*WINDOW_SIZE)
		{
			img_stab_if_R::myParamVec.erase(img_stab_if_R::myParamVec.begin());
		}
    }
/*
  else
  {
  while(myParamVec.size() <= 4*WINDOW_SIZE - 4)
  {
  myParamVec.push_back(0);
  }
  }
  */
	pA = pMyArray;
    for(int i =0; i < 4; i++)
    {
		img_stab_if_R::myParamVec.push_back(*pA);
		pA++;
    }

#if IS_DEBUG
    prints("\n--DEBUG: print array...\n\r");
    prints("**************************************\n\r");
    for(vector<kalmanType>::size_type  i =0; i < img_stab_if_R::myParamVec.size() ; i++)
    {
		prints("\t %lf", img_stab_if_R::myParamVec[i]);
    }
#endif

}


/* 
 *Caculate the new values of s theta and t based on myParamVec. 
 */
void img_stab_if_R::calcSmoothMotion(void)
{
    //create intermediate array.

#if IS_DEBUG
	prints("\n\rEnterning calcSmoothMotion function...");
#endif

	vector<kalmanType> tempArray;
    for(vector<kalmanType>::size_type  i = 0; i < img_stab_if_R::myParamVec.size() ; i++)
    {
		tempArray.push_back(myParamVec[i]);
#if IS_DEBUG
		prints("myParamVec[%d]: %lf\t", i, img_stab_if_R::myParamVec[i]);
#endif
    }
    vector<kalmanType> tempVecS;
    vector<kalmanType> tempVecTheta;
    vector<kalmanType> tempVecT1;
    vector<kalmanType> tempVecT2;

    for(vector<kalmanType>::size_type  i = 0; i < (WINDOW_SIZE) ; i++)
    {

		tempVecS.push_back(tempArray[i*4]);
		tempVecTheta.push_back(tempArray[i*4 + 1]);
		tempVecT1.push_back(tempArray[i*4 + 2]);
		tempVecT2.push_back(tempArray[i*4 + 3]);
    }
    // Now make one array with WINDOW_SIZE+1 and put the stuff inside.
    vector<kalmanType> copyVecS;
    vector<kalmanType> copyVecTheta;
    vector<kalmanType> copyVecT1;
    vector<kalmanType> copyVecT2;
    for(vector<kalmanType>::size_type  i = 0; i < (WINDOW_SIZE) ; i++)
    {
		copyVecS.push_back(tempVecS[i]);
		copyVecTheta.push_back(tempVecTheta[i]);
		copyVecT1.push_back(tempVecT1[i]);
		copyVecT2.push_back(tempVecT2[i]);
    }
    // append one more entry.
    copyVecS.push_back(1);
    copyVecTheta.push_back(0);
    copyVecT1.push_back(0);
    copyVecT2.push_back(0);

    //kalmanType thisS;
    kalmanType thisTheta;
    kalmanType thisT[2];

    //kalmanType finalS;
    //kalmanType finalTheta;
    //kalmanType finalT[2];


    for(vector<kalmanType>::size_type  i = WINDOW_SIZE+1; i > 1 ; i--)
    {
		copyVecS[i-2]= copyVecS[i-1]*tempVecS[i-2]; 
		copyVecTheta[i-2]= copyVecTheta[i-1] + tempVecTheta[i-2];
		thisTheta = copyVecTheta[i-2];
		//thisS = copyVecS[i-2];

		kalmanType thisTtoAdd[2];
		kalmanType thisTtoMult[2];

		thisTtoAdd[0] = copyVecT1[i-1];
		thisTtoAdd[1] = copyVecT2[i-1];
		thisTtoMult[0] = tempVecT1[i-2];
		thisTtoMult[1] = tempVecT2[i-2];

		kalmanType mat[4];
		mat[0] = cos(thisTheta);
		mat[1] = -sin(thisTheta);
		mat[2] = sin(thisTheta);
		mat[3] = cos(thisTheta);

#if 0
		prints("\r\t Mat: %lf %lf %lf %lf", mat[0], mat[1], mat[2], mat[3]);
		prints("\r\t Tadd : %ls %ls", thisTtoAdd[0], thisTtoAdd[1]);
		prints("\r\t Tmult: %lf %lf", thisTtoMult[0], thisTtoMult[1]);
		prints("\r\t multS: %lf", tempVecS[i-2]);
#endif
		kalmanType tempMult[2];
		tempMult[0] = tempVecS[i-2]*(mat[0]*thisTtoMult[0] + mat[1]*thisTtoMult[1]);
		tempMult[1] = tempVecS[i-2]*(mat[2]*thisTtoMult[0] + mat[3]*thisTtoMult[1]);

		thisT[0] = tempMult[0] + thisTtoAdd[0];
		thisT[1] = tempMult[1] + thisTtoAdd[1];

		copyVecT1[i-2]= copyVecT1[i-1] + thisT[0];
		copyVecT2[i-2]= copyVecT2[i-1] + thisT[1];

    }

    /*
     * Now push these values into a array to be used for filterning.
     */

    // construct the vector to hold the values.
    vector<kalmanType> filterVec;

    // Push the values into it.
    for(vector<kalmanType>::size_type  i = 0; i < copyVecS.size() - 1 ; i++)
    {
		filterVec.push_back(copyVecS[i]);
		filterVec.push_back(copyVecTheta[i]);
		filterVec.push_back(copyVecT1[i]);
		filterVec.push_back(copyVecT2[i]);
    }

    if(filterVec.size() >= 4*FILTER_SIZE)
    {
		while(filterVec.size() > 4*FILTER_SIZE)
		{
			filterVec.erase(filterVec.begin());
		}
    }
    else
    {   
		while(filterVec.size() < 4*FILTER_SIZE)
		{
			filterVec.push_back(0);
		}
    }

    // Filter it.

    for(vector<kalmanType>::size_type  i = FILTER_SIZE -1; i > 0 ; i--)
    {
		img_stab_if_R::myFilteredVec[0] = img_stab_if_R::myFilteredVec[0]
			+ filterVec[4*i];
		img_stab_if_R::myFilteredVec[1] = img_stab_if_R::myFilteredVec[1]
			+ filterVec[4*i + 1];
		img_stab_if_R::myFilteredVec[2] = img_stab_if_R::myFilteredVec[2]
			+ filterVec[4*i + 2];
		img_stab_if_R::myFilteredVec[3] = img_stab_if_R::myFilteredVec[3]
			+ filterVec[4*i + 3];

    }
    img_stab_if_R::myFilteredVec[0] = 
		img_stab_if_R::myFilteredVec[0]/FILTER_SIZE;
    img_stab_if_R::myFilteredVec[1] = 
		img_stab_if_R::myFilteredVec[1]/FILTER_SIZE;
    img_stab_if_R::myFilteredVec[2] = 
		img_stab_if_R::myFilteredVec[2]/FILTER_SIZE;
    img_stab_if_R::myFilteredVec[3] = 
		img_stab_if_R::myFilteredVec[3]/FILTER_SIZE;


#if IS_DEBUG

	/*
      cout<<"\nThisS "<<copyVecS[0];
      cout<<"\nThisTheta "<<copyVecTheta[0];
      cout<<"\nThisTheta "<<copyVecT1[0]<<copyVecT2[0];
      */

    for(vector<kalmanType>::size_type  i = 0; i < tempVecS.size() ; i++)
    {
		prints("\t copyVecS[%d] %lf ", i, copyVecS[i]);
    }

	prints("\n\r");
    for(vector<kalmanType>::size_type  i = 0; i < tempVecTheta.size() ; i++)
    {
		prints("\t copyVecTheta[%d] %lf \r\n", i, copyVecTheta[i]);
    }

	prints("\n\r");
	for(vector<kalmanType>::size_type  i = 0; i < tempVecT1.size() ; i++)
    {
		prints("\t tempVecT1[%d] %lf \r\n", i, tempVecT1[i]);
    }

	prints("\n\r");
	for(vector<kalmanType>::size_type  i = 0; i < tempVecT2.size() ; i++)
    {
		prints("\t temVecT2[%d] %lf \r\n", i, tempVecT2[i]);
    }
#endif


#if IS_DEBUG
	prints("\n\rDebug : size -> %d & %d", tempVecS.size(),tempArray.size());

	prints("\n--DEBUG: Printing s values.");
	for(vector<kalmanType>::size_type  i =0; i < tempVecS.size() ; i++)
	{
		prints("\t %lf", myVecS[i]);
	}
#endif

}


/*******************************************************************************
 * This Function run the whole IS module.
 *******************************************************************************/

void img_stab_if_R::runImageStabilizeR(void) 
{
    //int cnt;

    //ULONG lastVsyncTimeStamp;
    //ULONG lastMotionTimeStamp;
    prints("\n\rDEBUG : Enterning runImageStabilizeR()... \n\r");
    bool ifChangeInQuats = true;

#if IS_TEST

    kalmanType arrayTwoQuaternion[2*QUAT_LENGTH]={0.2e0, 2.9e0, 2.1e0, 3.2e0,
												  1.2e0, 3.2e0, 4.1e0, 1.1e0};

	/* Print the quaternion values. */
	prints("Quat : %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf\n\r",
			arrayTwoQuaternion[0], arrayTwoQuaternion[1], arrayTwoQuaternion[2],
			arrayTwoQuaternion[3], arrayTwoQuaternion[4], arrayTwoQuaternion[5],
			arrayTwoQuaternion[6], arrayTwoQuaternion[7]);

#endif
	kalmanType arrayCamInternalMatrix[18] = 
		{
			497.049180327869e0,	0,	384e0,
			0,	-238.583606557377e0,	800e0,
			0,	0,	1e0,
			497.049180327869e0,	0,	384e0,
			0,	-238.583606557377e0,	800e0,
			0,	0,	1e0
		};

    /* Here we update the arrayQuaternions */
    for(int i = 0; i < QUAT_LENGTH; i ++)
    {
		// pervious Quaternion = current Quaternion
   		arrayTwoQuaternion[i] = arrayTwoQuaternion[QUAT_LENGTH + i];
	}


	/***************************************************************************
     * Here, push the corrected quaternion into queue. This queue always contains current 
     * quaternion as well as previous quaternion.
     ***************************************************************************/

	for(int i = 0; i < QUAT_LENGTH; i++)
    {
//		prints("%lf ", pObjCamMotionManager->crctdQuaternion[i]);
		arrayTwoQuaternion[QUAT_LENGTH + i] = pObjCamMotionManager->crctdQuaternion[i];
    }
	prints("\n\r");
    prints("%lf \t %lf \t %lf \t %lf \r \n", arrayTwoQuaternion[4], arrayTwoQuaternion[5],
			arrayTwoQuaternion[6], arrayTwoQuaternion[7]);


	/**************************************************************************
	  ERROR HANDLING 
	  If there is no change in quaternions. return.
	  **************************************************************************/
    for(int i = 0; i < QUAT_LENGTH; i++)
    {
		if(arrayTwoQuaternion[i] == arrayTwoQuaternion[QUAT_LENGTH+i])
		{
			ifChangeInQuats = false;
			prints("\n\r No change in quaternion values. Returning...");
			return;
		}
    }

	/**************************************************************************
	  if quaternion is zero then return
	  **************************************************************************/
	int countZero1 = 0;
	int countZero2 = 0;
	for(int i = 0; i < QUAT_LENGTH; i++)
	{
		// check last 4 elements.
		if(0 == arrayTwoQuaternion[QUAT_LENGTH + i])
		{
			countZero1 = countZero1 + 1;
		}

		// Check first 4 elements.
		if(0 == arrayTwoQuaternion[i])
		{
			countZero2 = countZero2 + 1;
		}
	}
	// All 4 elements are zero. Return.
	if(3  < countZero1 || 3 < countZero2)
	{
		ifChangeInQuats = false;
		prints("\n\r Quaternion is of zero magnitute. Returning...");
		return;
	}


    /*
      TO DO: Extrapolate the quaternion using time stamp
      */


    /*
     * Create an array of camera matrix.
     * For testing : One can use artificial camera matrix.
     */
#if 0
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
		arrayCamInternalMatrix[i] = arrayCamInternalMatrix[K_MATRIX_LENGTH + i];
    }
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
		arrayCamInternalMatrix[K_MATRIX_LENGTH + i] = pObjCamMotionManager->pObjCamMotion->camInternalMat[i];
    }
#endif
    /*
     * CALLING FUNCTIONS to CALCULATE THE s, theta, and T.
     */

    //kalmanType* pA;
    //kalmanType* pQ;
    if(true == ifChangeInQuats)
    {   
		getHomographyMat(arrayTwoQuaternion, arrayCamInternalMatrix);

#if IS_DEBUG
		prints("\r\n--DEBUG : Printing Homography matrices. After getting homography\r\n");
		for(int i = 0 ; i < H_MATRIX_LENGTH; i++)
		{
			prints("IsHomoMat[%i]: %lf ", i, imgHomographyMat[i]);
		}
		prints("\r\n");
#endif

		// Decompose this homography matrix.
		decomposeHomographyMat(IsHomographyR::imgHomographyMat);

		/*
		 * this call will accumulate all the parameters into a queue of windowSize values.
		 */

		accParamsInQueue(IsHomographyR::imgSThetaTArray);

		/*
		 * This call will filter the values. filterLength can be changes in macro.
		 */

		calcSmoothMotion();
    }

    /*
     * Debug : Print the values here
     */
	prints("\n\r s : %lf Theta : %lf, T1 : %lf, T2 : %lf", myFilteredVec[0], myFilteredVec[1],
			myFilteredVec[2], myFilteredVec[3]);

#if 1
    /*
     * This part of the code will adjust the cropping parameters. The image is to
     * be cropped using the translation vector. Using theta (i.e. rotation of the
     * image) is not possible due to hardware limitations.
     * IT WILL RUN IF THERE IS ANY CHANGE IN CURRENT QUATERNION AND PREVIOUS
	 * QUATERNIUON.
     */

    /*
     * put the translation vector in a separete variable.
     */
    double transVec_x = imgSThetaTArray[2];
    double transVec_y = imgSThetaTArray[3];
    double centre_x = transVec_x;  // Change the values.
    double centre_y = transVec_y;  // Change the values.
    /*
     * Define a relation between screen resolution and translation vector.
     * ToDO : Make a macro.
     */
    if(true == ifChangeInQuats)
    {
		double scaleFactor = 1;
		double moveCentre_x = (centre_x * scaleFactor);
		double moveCentre_y = (centre_y * scaleFactor);

		/*
		 * call the function which update the cropping parameters.
		 */
		prints("\n\r Movement is : %lf %lf", moveCentre_x, moveCentre_y);
		UINT8 ucMonplId = 0;
		for (int i = 0; i < 3; i++)
		{
			monpl_adjustCropParameter(moveCentre_x, moveCentre_y, ucMonplId);
			ucMonplId++;
		}
    }
#endif

}

